#include "stdio.h"
#include "gsl/gsl_matrix.h"
#include "PowerFlowCalculation.h"
#include "iostream"
#include "readcsv.h"
#include "build_table.h"

gsl_matrix* array2gsl(double *data,int row,int col){
    gsl_matrix *m=gsl_matrix_alloc(row,col);
    for(int i=0;i<row;i++){
        for(int j=0;j<col;j++){
            gsl_matrix_set(m,i,j,*data);
            data++;
        }
    }
    return m;
}
gsl_matrix* gsl_matrix_ones(int raw,int col,double k){
    gsl_matrix* one=gsl_matrix_alloc(raw,col);
    for(int i=0;i<raw;i++){
        for(int j=0;j<col;j++){
            gsl_matrix_set(one,i,j,k);
        }
    }
    return one;
}
static Node m_Node_Initial[100000];

int main()
{
    vector<Branch> branch;
    vector<Branch> switchBranch;
    vector<vector<int>> table;
    vector<vector<int>> minPlossTable;
    int min_ploss;
   

    readBranch(branch,"HVDNbrchInf.txt");
    buildSwitchBranch(branch,switchBranch,"HVDNSwitchInf.txt");
 

    build(branch,3,4,table,switchBranch);
    return 0;
    
}

// int main(){
//     const int ND_COUNT=100;
//     const double CENTER_PARAM=0.05;
//     const double STEP_UNIT=1.02;
//     const double DEF_PRESICION=0.1;

//     //read_from_csv_double("Input_Data(1-24)_branch2.csv");
//     LoadData();

//     RE_IEEE14 ri=RE_IEEE14_data();
//     Form_IPM fi=Form_IPM_data16(ri.PQ_Node,ri.PV_Node,ri.ESS_Node,ri.Swing_Node,ri.Node_Num);
//     FormGB fg=FormGB_former(ri.TransFormer_Branch,ri.Normal_Branch,ri.PQ_Node,ri.PV_Node,ri.Node_Num,fi.m_Node);
//     Power_Calculation_Node pcn=Power_Calculation(fg.G,fg.B,ri.PQ_Node,ri.PV_Node,ri.ESS_Node,ri.Swing_Node,fi.Node_Num,fi.m_Node);

    
//     for(int i=0;i<ri.Node_Num;i++){
//         m_Node_Initial[i]=fi.m_Node[i];
//     }

//     double PLoss=PLoss_Calculation(fi.m_Node,fi.Node_Num,fg.G,fg.B);
//     //printf("ploss=%.20lf\n",PLoss);
//     Node* m_Node=fi.m_Node;
//     gsl_matrix* m_G=fg.G;
//     gsl_matrix* m_B=fg.B;
//     int Node_Num=fi.Node_Num;
//     int GenCount=fi.GenCount;
//     int nVaribles=fi.nVaribles;
//     int nEquals=fi.nEquals;
//     int nNequals=fi.nNequals;
//     double Total_PLoad=fi.Total_PLoad;
//     double Total_QLoad=fi.Total_QLoad;
//     gsl_matrix* voltagee=pcn.voltagee;
//     gsl_matrix* angle=pcn.angle;

//     gsl_matrix* Gap=gsl_matrix_alloc(1,ND_COUNT);

//     /*******************************program init**************/
//     gsl_matrix* m_l=gsl_matrix_alloc(nNequals,1);
//     gsl_matrix* m_u=gsl_matrix_alloc(nNequals,1);
//     int j=0;
//     for(int i=0;i<Node_Num;i++){
//         if(m_Node[i].Type == 1){
//             gsl_matrix_set(m_l,j,0,m_Node[i].Qg-m_Node[i].QDnLmt);
//             gsl_matrix_set(m_u,j,0,-m_Node[i].Qg+m_Node[i].QUpLmt);
//             j++;

//         }
//     }

//     for(int i=0;i<Node_Num;i++){
//         if(m_Node[i].Type == 3){
//             gsl_matrix_set(m_l,j,0,m_Node[i].Pg-m_Node[i].QDnLmt);
//             gsl_matrix_set(m_u,j,0,-m_Node[i].Pg+m_Node[i].QUpLmt);
//             j++;

//         }
//     }

//     for(int i=0;i<Node_Num;i++){
//         gsl_matrix_set(m_l,j,0,m_Node[i].Vol-m_Node[i].VolDnLmt);
//         gsl_matrix_set(m_u,j,0,-m_Node[i].Vol+m_Node[i].VolUpLmt);
//         j++;
        
//     }


//     //     PRINT_MATRIX(m_l)
//     // printf("\n");
//     // PRINT_MATRIX(m_u)

//     gsl_matrix* m_z=gsl_matrix_ones(nNequals,1,1);
//     gsl_matrix* m_w=gsl_matrix_ones(nNequals,1,-0.5);
//     gsl_matrix* m_y=gsl_matrix_ones(nEquals,1,-0.01);

//     bool flag=false;
//     int nCount=0;
//     int test_ID=1;
//     int fGap_Old=0;
//     //printf("ploss=%lf\n",pcn.PLoss);
//     gsl_matrix* m_l_T=gsl_matrix_alloc(1,nNequals);
//     gsl_matrix* m_u_T=gsl_matrix_alloc(1,nNequals);
//     gsl_matrix* fGap_m=gsl_matrix_alloc(nNequals,nNequals);
//     gsl_matrix* temp2=gsl_matrix_alloc(nNequals,nNequals);
//     double m_fR=0;
//     double fGap=0;
//     gsl_matrix* Power_err=gsl_matrix_alloc(1,ND_COUNT);
//     gsl_matrix* P_PLoss=gsl_matrix_alloc(1,ND_COUNT);
//     gsl_matrix* Qg=gsl_matrix_alloc(1,Node_Num);
//     gsl_matrix* vol=gsl_matrix_alloc(1,Node_Num);

//     while((!flag)&&(nCount<=ND_COUNT)){
//         nCount++;

//         gsl_matrix_transpose_memcpy(m_l_T,m_l);

//         gsl_matrix_transpose_memcpy(m_u_T,m_u);

//         gsl_matrix_mul(m_l_T,m_z,fGap_m);
//         gsl_matrix_mul(m_u_T,m_w,temp2);
//         gsl_matrix_sub(fGap_m,temp2);

//         fGap=gsl_matrix_get(fGap_m,0,0);
        
//         gsl_matrix_set(Gap,0,nCount-1,fGap);
        
//         if(fGap<DEF_PRESICION){
//             flag=true;

//             break;
//         }
//         m_fR=CENTER_PARAM * fGap/2.0/nNequals;
//         ModifyNodeValue(m_Node,m_G,m_B,Node_Num);

//         gsl_matrix* Jocabi_H=EqualJacobiMatrix(m_Node,m_G,m_B,Node_Num,GenCount);
//         gsl_matrix* mnode=gsl_matrix_alloc(35,14);
//         mnode->data=&m_Node[0].Type;


//         gsl_matrix* Hession_H=EqualHeisenMatrix(m_Node,m_G,m_B,m_y,Node_Num,GenCount,nEquals,nVaribles);

//         gsl_matrix* Jocabi_G=InequalJacobiMatrix(GenCount,Node_Num);
       
//         gsl_matrix* Grads_obj=GradsMatrix(m_Node,m_G,m_B,Node_Num,GenCount);
//         gsl_matrix* Hession_obj=GradsHeisenMatrix(m_Node,m_G,m_B,Node_Num,GenCount);
//         //PRINT_MATRIX(Hession_obj)
//         gsl_matrix* L=gsl_matrix_alloc(nNequals,nNequals);
//         gsl_matrix* Z=gsl_matrix_alloc(nNequals,nNequals);
//         gsl_matrix* U=gsl_matrix_alloc(nNequals,nNequals);
//         gsl_matrix* W=gsl_matrix_alloc(nNequals,nNequals);

//         for(int i=0;i<nNequals;i++){
//             gsl_matrix_set(L,i,i,gsl_matrix_get(m_l,i,0));
//             gsl_matrix_set(U,i,i,gsl_matrix_get(m_u,i,0));
//             gsl_matrix_set(Z,i,i,gsl_matrix_get(m_z,i,0));
//             gsl_matrix_set(W,i,i,gsl_matrix_get(m_w,i,0));
//         }


// /**************** best solve*******************/
//         gsl_matrix* L_x=gsl_matrix_alloc(nVaribles,1);
//         gsl_matrix_memcpy(L_x,Grads_obj);
//         gsl_matrix* temp1=gsl_matrix_alloc(nVaribles,1);
//         gsl_matrix_mul(Jocabi_H,m_y,temp1);
//         gsl_matrix* temp2=gsl_matrix_alloc(nNequals,1);
//         gsl_matrix_memcpy(temp2,m_z);
//         gsl_matrix_add(temp2,m_w);
//         gsl_matrix* temp3=gsl_matrix_alloc(nVaribles,1);
//         gsl_matrix_mul(Jocabi_G,temp2,temp3);
//         gsl_matrix_sub(L_x,temp1);
//         gsl_matrix_sub(L_x,temp3);

//         //PRINT_MATRIX(L_x)
//         gsl_matrix* L_y=gsl_matrix_alloc(nEquals,1);
//         int k=0;
//         for(int i=0;i<Node_Num;i++){
//             if(m_Node[i].Type!=2){
//                 gsl_matrix_set(L_y,k,0,m_Node[i].Pg-m_Node[i].Pl-m_Node[i].P);
//                 k++;
//             }
//         }
//         gsl_matrix_set(Power_err,0,nCount-1,abs(gsl_matrix_max(L_y)));
//         gsl_matrix* L_z=gsl_matrix_alloc(nNequals,1);
//         gsl_matrix* L_w=gsl_matrix_alloc(nNequals,1);

//         k=0;
//         for(int i=0;i<Node_Num;i++){
//             if(m_Node[i].Type==1){
//                 gsl_matrix_set(L_z,k,0,m_Node[i].Qg-m_Node[i].QDnLmt-gsl_matrix_get(m_l,k,0));
//                 gsl_matrix_set(L_w,k,0,m_Node[i].Qg-m_Node[i].QUpLmt+gsl_matrix_get(m_u,k,0));
//                 k++;
//             }
//         }
//         for(int i=0;i<Node_Num;i++){
//             gsl_matrix_set(L_z,k,0,m_Node[i].Vol-m_Node[i].VolDnLmt-gsl_matrix_get(m_l,k,0));
//             gsl_matrix_set(L_w,k,0,m_Node[i].Vol-m_Node[i].VolUpLmt+gsl_matrix_get(m_u,k,0));
//             k++;
//         }


        

//         gsl_matrix* L_l = gsl_matrix_alloc(nNequals,1);
//         gsl_matrix* L_u = gsl_matrix_alloc(nNequals,1);

//         L_l = gsl_matrix_mul(gsl_matrix_mul(L,Z),gsl_matrix_ones(nNequals,1,1));
//         gsl_matrix_sub(L_l,gsl_matrix_ones(nNequals,1,m_fR));
//         L_u = gsl_matrix_mul(gsl_matrix_mul(U,W),gsl_matrix_ones(nNequals,1,1));
//         gsl_matrix_add(L_u,gsl_matrix_ones(nNequals,1,m_fR));

//         gsl_matrix* Hession=gsl_matrix_alloc(nVaribles,nVaribles);

//         gsl_matrix_memcpy(Hession,Hession_H);
//         gsl_matrix_sub(Hession,Hession_obj);

//         gsl_matrix* Hession_sub=gsl_matrix_alloc(nNequals,nNequals);
//         gsl_matrix_memcpy(Hession_sub,gsl_matrix_mul(GetInverse(L),Z));
//         gsl_matrix_sub(Hession_sub,gsl_matrix_mul(GetInverse(U),W));
//         gsl_matrix* Jocabi_G_tr=gsl_matrix_alloc(Jocabi_G->size2,Jocabi_G->size1);
//         gsl_matrix_transpose_memcpy(Jocabi_G_tr,Jocabi_G);
//         gsl_matrix_sub(Hession,gsl_matrix_mul(gsl_matrix_mul(Jocabi_G,Hession_sub),Jocabi_G_tr));
    
//         gsl_matrix* HH = gsl_matrix_alloc(nVaribles+nEquals,nVaribles+nEquals);

//         gsl_matrix* Jocabi_H_tr=gsl_matrix_alloc(Jocabi_H->size2,Jocabi_H->size1);
//         gsl_matrix_transpose_memcpy(Jocabi_H_tr,Jocabi_H);


//         for(int i=0;i<nVaribles;i++)
//         {
//             for(int j=0;j<nVaribles;j++)
//             {
//                 gsl_matrix_set(HH,i,j,gsl_matrix_get(Hession,i,j));
//             }
//         }
//         for(int i=0;i<Jocabi_H->size1;i++)
//         {
//             for(int j=0;j<Jocabi_H->size2;j++)
//             {
//                 gsl_matrix_set(HH,i,j+nVaribles,gsl_matrix_get(Jocabi_H,i,j));
//             }
//         }
//         for(int i=0;i<Jocabi_H_tr->size1;i++)
//         {
//             for(int j=0;j<Jocabi_H_tr->size2;j++)
//             {
//                 gsl_matrix_set(HH,i+nVaribles,j,gsl_matrix_get(Jocabi_H_tr,i,j));
//             }
//         }
        

//         gsl_matrix* L_xx=gsl_matrix_alloc(nVaribles,1);
//         // gsl_matrix* L_xx_temp1=gsl_matrix_alloc(L_l->size1,L_l->size2);
//         // gsl_matrix* L_xx_temp2=gsl_matrix_alloc(L_u->size1,L_u->size2);
//         // L_xx_temp1=gsl_matrix_add2(L_l,gsl_matrix_mul(Z,L_z));
//         // L_xx_temp2=gsl_matrix_sub2(L_u,gsl_matrix_mul(W,L_w));
//         // L_xx=gsl_matrix_add2(L_x,gsl_matrix_mul(Jocabi_G,gsl_matrix_add2(gsl_matrix_mul(GetInverse(L),L_xx_temp1),gsl_matrix_mul(GetInverse(U),L_xx_temp2))));



//         L_xx=gsl_matrix_add2(L_x,gsl_matrix_mul(Jocabi_G,gsl_matrix_add2(gsl_matrix_mul(GetInverse(L),gsl_matrix_add2(L_l,gsl_matrix_mul(Z,L_z))),gsl_matrix_mul(GetInverse(U),gsl_matrix_sub2(L_u,gsl_matrix_mul(W,L_w))))));

//         gsl_matrix* xy_right=gsl_matrix_alloc(L_xx->size1+L_y->size1,L_xx->size2);
//         for(int i=0;i<L_xx->size1;i++)
//         {
//             gsl_matrix_set(xy_right,i,0,gsl_matrix_get(L_xx,i,0));
//         }
//         for(int i=0;i<L_y->size1;i++)
//         {
//             gsl_matrix_set(xy_right,i+L_xx->size1,0,-gsl_matrix_get(L_y,i,0));
//         }


//         gsl_matrix* xy=gsl_matrix_alloc(xy_right->size1,xy_right->size2);
//         xy=gsl_matrix_mul(GetInverse(HH),xy_right);

//         gsl_matrix* delt_x=gsl_matrix_alloc(nVaribles,1);
//         for(int i=0;i<nVaribles;i++)
//         {
//             gsl_matrix_set(delt_x,i,0,gsl_matrix_get(xy,i,0));
//         }
//         gsl_matrix* delt_y=gsl_matrix_alloc(nEquals,1);
//         for(int i=0;i<nEquals;i++)
//         {
//             gsl_matrix_set(delt_y,i,0,gsl_matrix_get(xy,i+nVaribles,0));
//         }
        
//         gsl_matrix* delt_u=gsl_matrix_alloc(L_w->size1,1);
//         delt_u=gsl_matrix_sub2(gsl_matrix_sub2(gsl_matrix_alloc(L_w->size1,1),L_w),gsl_matrix_mul(Jocabi_G_tr,delt_x));


//         gsl_matrix* delt_w=gsl_matrix_alloc(U->size1,1);
//         delt_w=gsl_matrix_mul(GetInverse(U),gsl_matrix_add2(L_u,gsl_matrix_mul(W,delt_u)));
//         delt_w=gsl_matrix_sub2(gsl_matrix_alloc(U->size1,1),delt_w);

//         gsl_matrix* delt_l=gsl_matrix_alloc(L_z->size1,L_z->size2);
//         delt_l=gsl_matrix_sub2(gsl_matrix_sub2(L_z,L_w),delt_u);

//         gsl_matrix* delt_z=gsl_matrix_alloc(L->size1,1);
//         delt_z=gsl_matrix_mul(GetInverse(L),gsl_matrix_add2(L_l,gsl_matrix_mul(Z,delt_l)));
//         delt_z=gsl_matrix_sub2(gsl_matrix_alloc(L->size1,1),delt_z);
        
//         double fp=1.0;
//         double fd=1.0;
//         gsl_matrix* f1=gsl_matrix_alloc(1,nNequals);
//         for(int i=0;i<nNequals;i++)
//         {
//             double f = -(gsl_matrix_get(m_l,i,0)/gsl_matrix_get(delt_l,i,0));
//             gsl_matrix_set(f1,0,i,-(gsl_matrix_get(m_l,i,0)/gsl_matrix_get(delt_l,i,0)));
//             if(f>0 && f<fp)
//             {
//                 fp=f;
//             }
//         }
        
        

//         gsl_matrix* f2=gsl_matrix_alloc(1,nNequals);
//         for(int i=0;i<nNequals;i++)
//         {
//             double f = -(gsl_matrix_get(m_u,i,0)/gsl_matrix_get(delt_u,i,0));
//             gsl_matrix_set(f2,0,i,-(gsl_matrix_get(m_u,i,0)/gsl_matrix_get(delt_u,i,0)));
//             if(f>0 && f<fp)
//             {
//                 fp=f;
//             }
//         }
        

//         gsl_matrix* f3=gsl_matrix_alloc(1,nNequals);
//         for(int i=0;i<nNequals;i++)
//         {
//             double f = -(gsl_matrix_get(m_z,i,0)/gsl_matrix_get(delt_z,i,0));
//             gsl_matrix_set(f3,0,i,-(gsl_matrix_get(m_z,i,0)/gsl_matrix_get(delt_z,i,0)));
//             if(f>0 && f<fp)
//             {
//                 fd=f;
//             }
//         }


//         gsl_matrix* f4=gsl_matrix_alloc(1,nNequals);
//         for(int i=0;i<nNequals;i++)
//         {
//             double f = -(gsl_matrix_get(m_w,i,0)/gsl_matrix_get(delt_w,i,0));
//             gsl_matrix_set(f4,0,i,-(gsl_matrix_get(m_w,i,0)/gsl_matrix_get(delt_w,i,0)));
//             if(f>0 && f<fp)
//             {
//                 fd=f;
//             }
//         }

//         fp*=STEP_UNIT;
//         fd*=STEP_UNIT;
        

//         k=0;

//         for(int i=0;i<Node_Num;i++)
//         {
//             if(m_Node[i].Type == 1)
//             {
//                 m_Node[i].Qg += fp*gsl_matrix_get(delt_x,k,0);
//                 gsl_matrix_set(Qg,0,k,m_Node[i].Qg);
//                 k++;
//             }
//         }

        
//         for(int i=0;i<Node_Num;i++)
//         {
//             m_Node[i].Vol += fp*gsl_matrix_get(delt_x,k,0);
            
//             gsl_matrix_set(vol,0,i,m_Node[i].Vol);
            
//             k++;
//         }
//         for(int i=0;i<Node_Num;i++)
//         {
//             if(m_Node[i].Type != 2)
//             {
//                 m_Node[i].Angle+=fp*gsl_matrix_get(delt_x,k,0);
//                 k++;
//             }
//             gsl_matrix_set(angle,0,i,m_Node[i].Angle);
//         }


//         gsl_matrix_add(m_l,gsl_matrix_scale2(delt_l,fp));
//         gsl_matrix_add(m_u,gsl_matrix_scale2(delt_u,fp));


//         gsl_matrix_add(m_y,gsl_matrix_scale2(delt_y,fd));
//         gsl_matrix_add(m_z,gsl_matrix_scale2(delt_z,fd));
//         gsl_matrix_add(m_w,gsl_matrix_scale2(delt_w,fd));

//         PLoss=PLoss_Calculation(m_Node,Node_Num,m_G,m_B);
//         gsl_matrix_set(P_PLoss,0,nCount-1,PLoss);


//     }
    
//     //PRINT_MATRIX(P_PLoss)
//     double P_slack = 0;
//     double Q_sum=0;

//     if(flag==true)
//     {
//         for(int i=0;i<Node_Num;i++)
//         {
//             double f=m_Node[Node_Num-1].Angle-m_Node[i].Angle;
//             P_slack+=m_Node[Node_Num-1].Vol*m_Node[i].Vol*(gsl_matrix_get(m_G,Node_Num-1,i)*cos(f)+gsl_matrix_get(m_B,Node_Num-1,i)*sin(f));
//         }
//         P_slack+=m_Node[Node_Num-1].Pl;
//         double QLoss=0;
//         for(int i=0;i<Node_Num;i++)
//         {
//             for(int j=0;j<Node_Num;j++)
//             {
//                 double f=m_Node[i].Angle - m_Node[j].Angle;
//                 QLoss+=m_Node[i].Vol*m_Node[j].Vol*(gsl_matrix_get(m_G,i,j)*sin(f)-gsl_matrix_get(m_B,i,j)*cos(f));
//             }
//         }
//         for(int i=0;i<Qg->size1;i++)
//         {
//             for(int j=0;j<Qg->size2;j++)
//             {
//                 Q_sum+=gsl_matrix_get(Qg,i,j);
//             }
//         }

//         gsl_matrix* Voltage=v14(m_Node_Initial,Node_Num);
//         //PRINT_MATRIX(Voltage)
//         printf("Qloss=%lf\n",QLoss);

//     }


//     return 0;
// }




